function [pL1S1,pL1S2,pL1S3]=servoL1(AngShoulder,AngLong,AngFoot)
    S1=pi/2+AngShoulder;
    S2=-AngLong;
    S3=AngFoot+pi;
    %计算单腿三个舵机角度
    pL1S1=round(S1*2000/pi+500);
    if (pL1S1<500)
        pL1S1=500;
    elseif (pL1S1>2500)
        pL1S1=2500;
    end
    %
    pL1S2=round(S2*2000/pi+500);
    if (pL1S2<500)
        pL1S2=500;
    elseif (pL1S2>2500)
            pL1S2=2500;
    end
    %
    pL1S3=round(S3*2000/pi+500);
    if (pL1S3<500)
        pL1S3=500;
    elseif (pL1S3>2500)
            pL1S3=2500;
    end
end